'''
####
Date : 2024-10-17
Notes: 示教器server
Test: 通讯格式
1. 修改代码执行部分代码---解析代码长度(通讯接收缓存)
    client_socket.recv(10240, 0x40),第二个参数 阻塞/与否
2. 增添视觉通讯功能代码
3. KBot通讯功能完善
4. 0.1.0版本--2024.10.29
####

Modify: 
1. kata新库更换--详见self.robot_control类


####
'''

# -*- coding: utf-8 -*-

import json
import logging
import os

import socket
import struct
import threading
import time

import modbus_tk.defines as cst
import config

from robotControl_ import robot_Control

from processLua_ import LuaProcessor
from processModbusTCP__ import process_Master
from processModbusRTU_ import rtu_Master

process_master = process_Master()
rtu_master = rtu_Master()

robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)

GB_dict = {  
    f"GB{i:02d}": {"address": f"{i-1}", "value": False}  
    for i in range(1, 43)  
}  

GI_dict = {  
    f"GI{i:02d}": {"address": f"{300 + (i-1)}", "value": 0}  
    for i in range(1, 43)  
}  

GD_dict = {  
    f"GD{i:02d}": {"address": f"{400 + (i-1)*2}", "value": 0.0}  
    for i in range(1, 43)  
}

GP_dict = {  
    f"GP{i:02d}": {"address": f"{500 + (i-1)*2}", "value": 00.00}  
    for i in range(1, 43)  
} 

repository1 = []
repository2 = []
repository3 = []
repository4 = []

class VSR_Server:

    def __init__(self) -> None: 
        ###初始化TCP服务
        self.vsrServer()

        ### 初始化全局变量参数
        self.connect_flag = 0
        self.IO = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.select_com_serial = False
        self.robotType_label = 0
        self.threads = []
        self.stopLuaDemo = False
        self.client_socket = None
        self.KBotConnectStatus = False

        self.robot_control = robot_Control()
        self.lua_robotLua = LuaProcessor()
        self.lua_thre = threading.Thread(target = self.lua_robotLua.run)
        self.lua_thre.start()
        time.sleep(0.055)
        self.lua_robotLua.runningLua = False
        self.lock_object = threading.RLock()

    def set_stop(self):
        self.stopLuaDemo = True

    def get_stop(self):
        return self.stopLuaDemo

    def vsrServer(self):
        self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)

        self.tcp_socket.bind(('0.0.0.0', 8090))
        self.tcp_socket.listen(10)

    def float2bytes(self,f):
        #将浮点数打包成32位2进制数据
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]      

    def run(self):  
        try:  
            while True:  
                self.client_socket, self.client_addr = self.tcp_socket.accept()
                print(f"Connected by {self.client_addr}")
                if self.client_socket:
                    self.KBotConnectStatus = True
                else:
                    self.KBotConnectStatus = False
                robot_logger.info(f"Connected by {self.client_addr}")

                # 传递必要的参数给线程函数  
                def thread_function():  
                    while True:  
                        self.send_data_process()  
                        time.sleep(0.05)  # 50毫秒  
  
                # 创建并启动线程  
                thread_sendData = threading.Thread(target=thread_function)  
                thread_sendData.start()

                time.sleep(0.05)
                inner_thread = threading.Thread(target=self.inner_thread_func)
                inner_thread.start()
                ### 为每个客户端启动一个新的线程  
                # client_thread = threading.Thread(target=self.inner_thread_func, args=(client_socket, client_addr))  
                # client_thread.start()  
                # self.threads.append(client_thread)  

        except KeyboardInterrupt:  
            robot_logger.info("Server is shutting down...")  
            # 尝试优雅地关闭所有线程和套接字  
            self.tcp_socket.close()  
            for thread in self.threads:  
                thread.join()  # 等待所有线程完成  
  
    def handle_client(self):  
        try:  
            inner_thread = threading.Thread(target=self.inner_thread_func)
            inner_thread.start()
            # inner_thread.join()
        
        except Exception as e:
            print(f"handle_client is error is {e}")

    def inner_thread_func(self,):
        try:  
            while True:
                try:
                    recv_data = self.client_socket.recv(10240, 0x40) 
                    if not recv_data: 
                        print("---- recv_data is None!!!!!!!!!!!") 
                        break  ### Client disconnected
    
                    recv_str = recv_data.decode('utf-8')
                    # print(f"---- recv_data is {recv_str}----")
                    self.parse_data(recv_str)
                        
                except BlockingIOError as e:
                    recv_data = None
                    time.sleep(0.01)

        finally:  
            self.client_socket.close()  
            robot_logger.info(f"Disconnected {self.client_addr}")  


    def recvData2json(self, data_ori, client_socket):
        try:
            count = 0
            for char in data_ori:
                if char == '}':
                    count += 1

            if count == 2:
                self.downloadMode = True
                index = data_ori.find('}')
                if index != -1:
                    first_part = data_ori[:index+1]
                    second_part = data_ori[index+1:]

                try:
                    if "StartLua" in first_part:
                        if isinstance(second_part, str):  
                            data = json.loads(second_part)
                            
                        self.str2luaFile(data)
                        time.sleep(0.05)
                        ### Lua解释器-----new
                        # self.lua_robotLua.lua_execute()
                        self.lua_robotLua.runningLua = True
                        ### Lua解释器-----original
                        # self._lua.process_instructions()

                    elif "StopLua" in first_part:
                        self.robot_control.robot_stop(com_Num = 1)
                        self.robot_control.robot_stop(com_Num = 2)
                        self.robot_control.robot_stop(com_Num = 3)                 
                        self.lua_robotLua.stop_process()
                        rtu_master.rtu_server.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address=10, output_value=[0,0,0,0,0,0,0,0])

                    elif "luaFile" in first_part:
                        if isinstance(second_part, str):  
                            data = json.loads(second_part)
                        self.str2luaFile(data)
                    
                    elif "GetLua" in first_part:
                        try:
                            self.file_path = '/home/marvsmart/factory/driver_KT/download.lua'
                            with open(self.file_path, 'r', encoding='utf-8') as file:
                                text = file.read()
                            data_size = len(text)
                            packet_type = "luaFile"
                            packet = self.build_packet(data_size, packet_type, text)
                            print(f"2024-9-10 send lua file {packet}")
                            client_socket.sendall(packet)
                        
                        except FileNotFoundError:
                            text = "File not found"
                        except Exception as e:
                            text = f"An error occurred: {e}"
                
                except json.decoder.JSONDecodeError as e:
                    print(f"JSON 解析错误: {e}")

            else:
                self.downloadMode = False
                index = data_ori.find('}')
                if index != -1:
                    first_part = data_ori[:index+1]
                    second_part = data_ori[index+1:]

                # 如果 data 是一个字符串，则将其转换为字典  
                if isinstance(second_part, str):  
                    data = json.loads(second_part)  
                    
                # 假设 data 是一个字典，并且包含你需要的键  
                if 'deviceData' in data:  
                    device_data = data['deviceData']
                    status = {  
                        "cmdtype": data.get("cmdtype", None),  
                        "robotCom": data.get("robotCom", None),
                        "robotType": data.get("robotType", None),
                        "sportsType":device_data.get("sportsType", ""),
                        "Offs":device_data.get("Offs", ""),
                        "sports": device_data.get("sports", ""),
                        "move_Mode":device_data.get("move_Mode", ""),
                        "cmd": device_data.get("cmd", ""),  
                        "speed": device_data.get("speed", None),  
                        "step": device_data.get("step", None),
                        "type": device_data.get("type", None),
                        "data": device_data.get("data", None)
                    }  
                    return status  
                else:
                    return None

        except Exception as e:  
            print(f"recvData2json error is {e}")  
            return None  # 重新抛出异常  

    def str2luaFile(self, data):
        luaData = data["luaData"]
        cleaned_data = luaData.replace('\\n', '\n').replace('\\"', '"')
        
        pointList = data["pointList"] 
        formatted_strings = [f"{name} = [{', '.join(map(str, values))}]" for name, *values in pointList]  
        
        # 将生成的字符串列表合并为一个大的字符串，并添加换行符  
        result_string = '\n'.join(formatted_strings)  

        # 如果你想要将这个字符串保存到文件中，可以这样做：  
        with open('/home/marvsmart/factory/driver_KT/download.lua', 'w') as file:
            file.write(result_string)
            file.write('\n') 
            file.write(cleaned_data)  

    def parse_data(self, recv_data):
        try:
            _parseOrd = self.recvData2json(recv_data, self.client_socket)
            # print(f"---- parseOrder is {_parseOrd} ----")
            if _parseOrd is not None :
                if _parseOrd['robotCom'] == 'com1':
                    self.select_com_serial = 1
                elif _parseOrd['robotCom'] == 'com2':
                    self.select_com_serial = 2
                elif _parseOrd['robotCom'] == 'usb1':
                    self.select_com_serial = 3
                
                self.robotType_label = 1
                if _parseOrd['robotType'] == 'KT-R6':
                    self.robotType_label = 1
                elif _parseOrd['robotType'] == 'KT-R4':
                    self.robotType_label = 2
                
                command = _parseOrd["cmd"]
                
                gcode = f"M50\r\n"
                
                self.robot_control.send_msg(gcode, com_flag = 1)
                time.sleep(0.1)
                self.robot_control.send_msg(gcode, com_flag = 2)
                time.sleep(0.1)

                #### 示教指令模式 ####
                if _parseOrd["cmdtype"] == "M_mode" or _parseOrd["cmdtype"] == "A_mode":
                    
                    if _parseOrd["sports"] is not None:
                        data_type = _parseOrd["sports"]
                    else:
                        data_type = "M21"
                    
                    if _parseOrd["sportsType"] is not None:
                        sportsType = _parseOrd["sportsType"]
                    else:
                        sportsType = "continuous"
                    
                    if _parseOrd["speed"] is not None:
                        speed = _parseOrd["speed"]
                    else:
                        speed = 2000
                    
                    if _parseOrd["step"] is not None:
                        self.step = _parseOrd["step"]
                    else:
                        self.step = 5

                    gcode = f"M50\r\n"
                    self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
                    time.sleep(0.1)
                    self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
                    if sportsType == "continuous":
                        if data_type == 'M21':
                            if "up" in command:
                                gcode = f"!\n"
                                if self.select_com_serial == 1:
                                    self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
                                
                                elif self.select_com_serial == 2:
                                    self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
                                
                                else:
                                    self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 3)
                                time.sleep(0.6)
                                gcode = f"%\n"
                            elif "down" in command : 
                                if self.select_com_serial == 2:
                                    s_data = self.robot_control.status_global_
                                elif self.select_com_serial == 1:
                                    s_data = self.robot_control.status_global
                                else:
                                    s_data = self.robot_control.status_global_3
                                
                                # if s_data["state"] == "Alarm" and command != "restart":
                                #     self.lock_object.acquire()
                                #     data = "Robot's status is Alarm, Please Reset!"
                                #     send_data = {"return":data}
                                #     send_data = json.dumps(send_data)+"\r\n"
                                #     if self.select_com_serial == 1:
                                #         self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
                                    
                                #     elif self.select_com_serial == 2:
                                #         self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
                                    
                                #     else:
                                #         self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 3)
                                #     time.sleep(0.05)
                                #     self.lock_object.release()
                                #     raise Exception("Please Reset!")
                                
                                angle_current = s_data["angle"]
                                angle_current = [int(i) for i in angle_current]
                                
                                ### 六轴机器人轴角限位值
                                if self.robotType_label == 1:
                                    ## 根据不同的命令执行不同的操作
                                    if command == "1_+_down":
                                        gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "1_-_down":
                                        gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "2_+_down":
                                        gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "2_-_down":
                                        gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "3_+_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "3_-_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "4_+_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A140 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[4],angle_current[5], speed)
                                    elif command == "4_-_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[4],angle_current[5], speed)
                                    elif command == "5_+_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[3],angle_current[5], speed)
                                    elif command == "5_-_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-190 C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[3],angle_current[5], speed)
                                    elif command == "6_+_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[3],angle_current[4], speed)
                                    elif command == "6_-_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[3],angle_current[4], speed)
                                    
                                    self.lock_object.acquire()
                                    if self.select_com_serial == 1:
                                        self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
                                    
                                    elif self.select_com_serial == 2:
                                        self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
                                    
                                    else:
                                        self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 3)
                                    time.sleep(0.006)
                                    self.lock_object.release()
                                
                                ### 四轴机器人轴角限位值
                                elif self.robotType_label == 2:
                                    ## 根据不同的命令执行不同的操作
                                    if command == "1_+_down":
                                        gcode = "M21 G90 G01 X180 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "1_-_down":
                                        gcode = "M21 G90 G01 X-130 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "2_+_down":
                                        gcode = "M21 G90 G01 X{} Y45 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "2_-_down":
                                        gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "3_+_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z40 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "3_-_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z-10 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[3],angle_current[4],angle_current[5], speed)
                                    elif command == "4_+_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[4],angle_current[5], speed)
                                    elif command == "4_-_down":
                                        gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                                                angle_current[2],angle_current[4],angle_current[5], speed)
                                    
                            self.lock_object.acquire()
                            if self.select_com_serial == 1:
                                self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
                            
                            elif self.select_com_serial == 2:
                                self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
                            
                            else:
                                self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 3)
                            time.sleep(0.006)
                            self.lock_object.release()
                         

                    elif sportsType == "point":
                        if data_type == 'M21':
                            if "up" in command:
                                gcode = f"!\n"
                                if self.select_com_serial == 1:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 1)
                                
                                elif self.select_com_serial == 2:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 2)
                                
                                else:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 3)
                                time.sleep(0.5)
                                gcode = f"%\n"
                            elif "down" in command : 
                                if self.select_com_serial == 2:
                                    s_data = self.robot_control.status_global_
                                elif self.select_com_serial == 1:
                                    s_data = self.robot_control.status_global
                                else:
                                    s_data = self.robot_control.status_global_3
                                
                                # if s_data["state"] == "Alarm" and command != "restart":
                                #     self.lock_object.acquire()
                                #     data = "Robot's status is Alarm, Please Reset!"
                                #     gcode = f"M50\r\n"
                                #     if self.select_com_serial == 1:
                                #         self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 1)
                                    
                                #     elif self.select_com_serial == 2:
                                #         self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 2)
                                    
                                #     else:
                                #         self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 3)
                                #     self.lock_object.release()
                                #     raise Exception("Please Reset!")
                                
                                angle_current = s_data["angle"]
                                angle_current = [int(i) for i in angle_current] 
                                
                                ## 根据不同的命令执行不同的操作
                                if command == "1_+_down":
                                    gcode = f"M21 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
                                elif command == "1_-_down":
                                    gcode = f"M21 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
                                elif command == "2_+_down":
                                    gcode = f"M21 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
                                elif command == "2_-_down":
                                    gcode = f"M21 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
                                elif command == "3_+_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
                                elif command == "3_-_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
                                elif command == "4_+_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
                                elif command == "4_-_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
                                elif command == "5_+_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
                                elif command == "5_-_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
                                elif command == "6_+_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"
                                elif command == "6_-_down":
                                    gcode = f"M21 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
                                else:
                                    # 处理未知命令的情况
                                    pass

                                self.lock_object.acquire()
                                if self.select_com_serial == 1:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 1)
                                
                                elif self.select_com_serial == 2:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 2)
                                
                                else:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 3)
                                time.sleep(0.006)
                                self.lock_object.release()

                        elif data_type == 'M20':
                            
                            if "up" in command:
                                gcode = f"!\n"
                                if self.select_com_serial == 1:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 1)
                                
                                elif self.select_com_serial == 2:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 2)
                                
                                else:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 3)
                                time.sleep(0.5)
                                gcode = f"%\n"
                            
                            elif "down" in command : 
                                if self.select_com_serial == 2:
                                    s_data = self.robot_control.status_global_
                                elif self.select_com_serial == 1:
                                    s_data = self.robot_control.status_global
                                else:
                                    s_data = self.robot_control.status_global_3
                                
                                # if s_data["state"] == "Alarm" and command != "restart":
                                #     self.lock_object.acquire()
                                #     data = "Robot's status is Alarm, Please Reset!"
                                #     gcode = f"M50\r\n"
                                #     if self.select_com_serial == 1:
                                #         self.robot_control.send_msg(gcode, com_flag = 1)
                                    
                                #     elif self.select_com_serial == 2:
                                #         self.robot_control.send_msg(gcode, com_flag = 2)
                                    
                                #     else:
                                #         self.robot_control.send_msg(gcode, com_flag = 3)
                                #     self.lock_object.release()
                                #     raise Exception("Please Reset!")
                                
                                angle_current = s_data["coordinate"]
                                angle_current = [int(i) for i in angle_current]
                                ## 根据不同的命令执行不同的操作
                                if command == "1_+_down":
                                    gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
                                elif command == "1_-_down":
                                    gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
                                elif command == "2_+_down":
                                    gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
                                elif command == "2_-_down":
                                    gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
                                elif command == "3_+_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
                                elif command == "3_-_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
                                elif command == "4_+_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
                                elif command == "4_-_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
                                elif command == "5_+_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
                                elif command == "5_-_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
                                elif command == "6_+_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"
                                elif command == "6_-_down":
                                    gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
                                else:
                                    # 处理未知命令的情况
                                    pass

                                self.lock_object.acquire()
                                if self.select_com_serial == 1:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 1)
                                
                                elif self.select_com_serial == 2:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 2)
                                
                                else:
                                    self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 3)
                                time.sleep(0.006)
                                self.lock_object.release()
                    
                
				#### 点到点模式 P2P####
                elif _parseOrd["cmdtype"] == "A_Mode":
                    try:
                        gcode = f"M50\r\n"
                        self.robot_control.send_msg(gcode, com_flag = 1)
                        time.sleep(0.1)
                        self.robot_control.send_msg(gcode, com_flag = 2)
                        time.sleep(0.1)

                        state_robot1 = config.get_value("s1")
                        state_robot2 = config.get_value("s2")
                        state_robot3 = config.get_value("s3")

                        if state_robot1 == "Alarm" or state_robot2 == "Alarm"  or state_robot3 == "Alarm" and command != "restart":
                            self.lock_object.acquire()
                            data = "Robot's status is Alarm, Please Reset!"
                            send_data = {"return":data}
                            send_data = json.dumps(send_data)+"\r\n"
                            if self.select_com_serial == 1:
                                self.robot_control.send_msg(gcode, com_flag = 1)
                            
                            elif self.select_com_serial == 2:
                                self.robot_control.send_msg(gcode, com_flag = 2)
                            
                            elif self.select_com_serial == 3:
                                self.robot_control.send_msg(gcode, com_flag = 3)
                            self.lock_object.release()
                            raise Exception("Please Reset!")
                        
                        p2p = _parseOrd["data"]
                        x = p2p[0]
                        y = p2p[1]
                        z = p2p[2]
                        a = p2p[3]
                        b = p2p[4]
                        c = p2p[5]
                        d = p2p[6]
                        speed = _parseOrd["speed"]
                        move_Mode = _parseOrd["move_Mode"]
                        sports_ = _parseOrd["sports"]
                        if move_Mode and move_Mode == "G91": 
                            _is_relative = True
                        else:
                            _is_relative = False

                        if self.select_com_serial == 1:
                            _com = 1
                        elif self.select_com_serial == 2:
                            _com = 2
                        else:
                            _com = 3
                        
                        if sports_ == "M21":
                            self.robot_control.go_to_axis(x=x, y=y, z=z, a=a, b=b, c=c, d = d, com_Num = _com)
                        else:
                            self.robot_control.linear_interpolation_o(x=x, y=y, z=z, a=a, b=b, c=c, com_Num = _com, speed=speed, is_relative=_is_relative)
                    
                    except Exception as e:
                        print(e)               

                #### IO模式 IO_Mode####
                elif _parseOrd["cmdtype"] == "IO_Mode":
                    try:
                        #### KT-AI4控制器
                        outputType = _parseOrd["type"]
                        outputData = _parseOrd["data"]
                        if "DO" in outputType:
                            output_type_num = {"DO0":0, "DO1":1, "DO2":2, "DO3":3, "DO4":4, "DO5":5, "DO6":6, "DO7":7}
                            num = output_type_num.get(outputType, None)
                            if num is not None:
                                ### 读写寄存器的方式控制DO
                                print(f"outputData", outputData)
                                if outputData == "1+" or outputData == "1":
                                    rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=num+11, output_value=1)
                                    process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=num+28, output_value=1)
                                else:
                                    rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=num+11, output_value=0)
                                    process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=num+28, output_value=0)
                        elif "Motor1" in outputType:
                            if outputData == "1+":

                                rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=20, output_value=1)
                                process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=47, output_value=1)
                            elif outputData == "1-":
                                rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=20, output_value=2)
                                process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=47, output_value=2)
                            elif outputData == "0":
                                rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=20, output_value=0)
                                process_master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=47, output_value=0)

                    except Exception as e:
                        print("Class VSR_Server IO_Mode error: ", e)
                
                #### 特殊指令模式
                elif  _parseOrd["cmdtype"] == "C_Mode":
                    # gcode = f"M50\r\n"
                    # self.robot_control.send_msg(gcode, com_flag = 1)
                    # time.sleep(0.1)
                    # self.robot_control.send_msg(gcode, com_flag = 2)
                    # time.sleep(0.1)

                    state_robot1 = config.get_value("s1")
                    state_robot2 = config.get_value("s2")
                    state_robot3 = config.get_value("s3")

                    # if state_robot1 == "Alarm" or state_robot2 == "Alarm" and command != "restart":
                    #     self.lock_object.acquire()
                    #     data = "Robot's status is Alarm, Please Reset!"
                    #     send_data = {"return":data}
                    #     send_data = json.dumps(send_data)+"\r\n"
                    #     if self.select_com_serial == 1:
                    #         self.robot_control.send_msg(gcode, com_flag = 1)
                        
                    #     elif self.select_com_serial == 2:
                    #         self.robot_control.send_msg(gcode, com_flag = 2)
                        
                    #     elif self.select_com_serial == 3:
                    #         self.robot_control.send_msg(gcode, com_flag = 3)
                    #     self.lock_object.release()
                    #     raise Exception("Please Reset!")
                    
                    command = _parseOrd["cmd"]
                    if command is not None:
                        if command == "restart":
                            gcode = "$h\r\n"
                        
                        elif command == "stop":
                            gcode = "o117\r\n"

                        elif command == "start":
                            gcode = "M50\r\n"
                        
                        elif command == "zero":
                            gcode = "$M\r\n"
                            # gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
    
                        self.lock_object.acquire()
                        if self.select_com_serial == 2:
                            self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
                        elif self.select_com_serial == 1:
                            self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
                        else:
                            self.robot_control.send_msg(gcode, wait_idle= False, com_flag = 3)
                        time.sleep(0.006)
                        self.lock_object.release()
            
        except json.JSONDecodeError as e:  
            print(f"JSON解析错误: {e}")                             

    def _unlockRobot(self):
        try:
            print(f"the unlockrobto code is execute")
            gcode = "M50\r\n"
            if self.select_com_serial == 1:
                self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 1)
            elif self.select_com_serial == 2:
                self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 2)
            else:
                self.robot_control.send_msg(gcode, wait_idle= True, com_flag = 3)

        except Exception as e:
            print(f"_unlock robot error is {e}")

    def build_packet(self,data_size, packet_type, message_body):  
        # 构建报头  
        header = json.dumps({  
            "data_size": data_size,  
            "type": packet_type  
        }).encode('utf-8')
        
        # 计算报头长度（4位bit，即半个字节，需要转换为字节长度）  
        header_length = len(header)
        
        # 构建帧头
        # frame_header = struct.pack('>H', header_length)
        frame_header = header_length.to_bytes(4, 'little')
        
        # 完整的数据包  
        packet = frame_header + header + message_body.encode('utf-8')
        return packet 

    # 定义消息体  
    def create_message_body(self, controller, robot_type, state_robot1, angle_robot1, coordinate_robot1, robot_type_robot2, state_robot2, angle_robot2, coordinate_robot2, 
                            robot_type_robot3, state_robot3, angle_robot3, coordinate_robot3, io, 
                            pose_ModbusVar, bool_ModbusVar, int_ModbusVar, float_ModbusVar):  
        message_body = { 
            "KT-AI":controller,
            "robotData1":{
                "robot_type": robot_type,
                "state": state_robot1,
                "angle": angle_robot1,
                "coordinate": coordinate_robot1
            },
            "robotData2":{
                "robot_type": robot_type_robot2,
                "state": state_robot2,
                "angle": angle_robot2,
                "coordinate": coordinate_robot2
            },
            "robotData3":{
                "robot_type": robot_type_robot3,
                "state": state_robot3,
                "angle": angle_robot3,
                "coordinate": coordinate_robot3
            },
            "IO": io,
            "modbusData":{
                "pose":pose_ModbusVar,
                "bool":bool_ModbusVar,
                "int":int_ModbusVar,
                "float":float_ModbusVar,
            }  
        }
        data_str = json.dumps(message_body) + '\n'
        return data_str

    def robot_type_send(self):
        a = self.robot_control.robot_type_number 
        b = self.robot_control.robot_type_number_2
        c = "{}_{}".format(a, b)
        return c

    def get_robot_type(self, config_key):  
        type_value = config.get_value(config_key) 
        # print(f"processCommunicate_get_robot_type type {type_value}") 
        if type_value == 1:
            return "KT-R6"  
        elif type_value == 2:
            return "KT-R4"
        else:
            return "未连接" 

    def read_modbus_register(self, address):
        add = int(address)
        register_value = process_master.master_.execute(1,cst.READ_HOLDING_REGISTERS,starting_address=add,quantity_of_x=1) 
        return register_value 
    
    def write_modbus_register(self, address, value):
        add = int(address)
        # val = int(value)
        process_master.master_.execute(1,cst.WRITE_SINGLE_REGISTER,starting_address=add,output_value=value)

    def read_bool_modbus_register(self, address):
        add = int(address)
        register_value = process_master.master_.execute(1,cst.READ_COILS,starting_address=add,quantity_of_x=1)  
        return register_value 
    
    def write_bool_modbus_register(self, address, value):
        add = int(address)
        if value == "0":
            value = False
        else:
            value = True
        process_master.master_.execute(1,cst.WRITE_SINGLE_COIL,starting_address=add,output_value=value)

    def read_multi_modbus_register(self, address):
        add = int(address)
        register_value = process_master.master_.execute(1,cst.READ_HOLDING_REGISTERS,starting_address=add,quantity_of_x=2) 
        return register_value 

    def write_multi_modbus_register(self, address, value):
        add = int(address)
        def float_to_int16s(f):
            # 将浮点数打包成32位二进制数据
            b = struct.pack('f', f)
            # 将32位二进制数据拆分成两个16位二进制数据
            i1, i2 = struct.unpack('HH', b)
            return i2, i1
        value_new2, val_new1 = float_to_int16s(value)
        val = [value_new2, val_new1]
        process_master.master_.execute(1, cst.WRITE_MULTIPLE_REGISTERS, starting_address = add, output_value = val)

    # 更新全局变量的函数  
    def update_modbus_variables(self, GB_dict, GI_dict, GD_dict, GP_dict):
        def int_float(a,b):
            value=0
            try:
                z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
                z=z0+z1 #高字节在前 低字节在后
                value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
                if value is None:
                    return value
                if isinstance(value, float):
                    # 精确到小数点后两位数
                    return round(value , 2)
                else:
                    return value
            except BaseException as e:
                robot_logger.error(e,exc_info=True)
            return value

        # 更新GP_dict变量  
        for key, value_dict in GP_dict.items():  
            address = int(value_dict["address"])  
            current_value = self.read_multi_modbus_register(address)  
            if value_dict["value"] is not None and value_dict["value"] != current_value:  
                val= int_float(current_value[0], current_value[1])
                # self.write_multi_modbus_register(address, val)  
                value_dict["value"] = val  # 假设写入成功，更新字典中的值  
    
        # 更新GB_dict变量  
        for key, value_dict in GB_dict.items():  
            address = int(value_dict["address"])
            val = value_dict["value"]
            current_value = self.read_bool_modbus_register(address)[0]
            if current_value == 0:
                current_value = False
            else:
                current_value = True
            if value_dict["value"] is not None and value_dict["value"] != current_value: 
                value_dict["value"] = current_value
    
        # 更新GI_dict变量  
        for key, value_dict in GI_dict.items():  
            address = int(value_dict["address"])  
            current_value = self.read_modbus_register(address)[0]  
            if value_dict["value"] is not None and value_dict["value"] != current_value:  
                value_dict["value"] = current_value
    
        # 更新GD_dict变量  
        for key, value_dict in GD_dict.items():  
            address = int(value_dict["address"])  
            current_value = self.read_multi_modbus_register(address)
            va = value_dict["value"]
            if value_dict["value"] is not None and value_dict["value"] != current_value:
                val= int_float(current_value[0], current_value[1])
                # self.write_multi_modbus_register(address, val)  
                value_dict["value"] = val  # 假设写入成功，更新字典中的值  

    def send_data_process(self):
        try:
            controller = "KT-AI-4.0"
            Angle = list()
            Angle_2 = list()
            Angle_3 = list()
            Coordinate = list()
            Coordinate_2 = list()
            Coordinate_3 = list()
            state_robot1 = None
            state_robot2 = None
            state_robot3 = None

            Angle.append(config.get_value("j0"))  
            Angle.append(config.get_value("j1"))
            Angle.append(config.get_value("j2"))
            Angle.append(config.get_value("j3"))
            Angle.append(config.get_value("j4"))
            Angle.append(config.get_value("j5"))
            Angle.append(config.get_value("FO[1]"))

            Coordinate.append(config.get_value("c0"))
            Coordinate.append(config.get_value("c1"))
            Coordinate.append(config.get_value("c2"))
            Coordinate.append(config.get_value("c3"))
            Coordinate.append(config.get_value("c4"))
            Coordinate.append(config.get_value("c5"))
            Coordinate.append(config.get_value("FO[1]"))

            Angle_2.append(config.get_value("j7"))
            Angle_2.append(config.get_value("j8"))
            Angle_2.append(config.get_value("j9"))
            Angle_2.append(config.get_value("j10"))
            Angle_2.append(config.get_value("j11"))
            Angle_2.append(config.get_value("j12"))
            Angle_2.append(config.get_value("FO[2]"))

            Coordinate_2.append(config.get_value("c6"))
            Coordinate_2.append(config.get_value("c7"))
            Coordinate_2.append(config.get_value("c8"))
            Coordinate_2.append(config.get_value("c9"))
            Coordinate_2.append(config.get_value("c10"))
            Coordinate_2.append(config.get_value("c11"))
            Coordinate_2.append(config.get_value("FO[2]"))

            Angle_3.append(config.get_value("j14"))
            Angle_3.append(config.get_value("j15"))
            Angle_3.append(config.get_value("j16"))
            Angle_3.append(config.get_value("j17"))
            Angle_3.append(config.get_value("j18"))
            Angle_3.append(config.get_value("j19"))
            Angle_3.append(config.get_value("FO[3]"))

            Coordinate_3.append(config.get_value("c12"))
            Coordinate_3.append(config.get_value("c13"))
            Coordinate_3.append(config.get_value("c14"))
            Coordinate_3.append(config.get_value("c15"))
            Coordinate_3.append(config.get_value("c16"))
            Coordinate_3.append(config.get_value("c17"))
            Coordinate_3.append(config.get_value("FO[3]"))

            state_robot1 = config.get_value("s1")
            state_robot2 = config.get_value("s2")
            state_robot3 = config.get_value("s3")

            # 使用函数获取机器人类型  
            type_robot1 = self.get_robot_type("robot_type")  
            type_robot2 = self.get_robot_type("robot_type2")
            type_robot3 = self.get_robot_type("robot_type3")
            
            if Angle is not None:
                Angle = Angle
            else:
                Angle = [0,0,0,0,0,0,0]
            
            if Angle_2 is not None:
                Angle_2 = Angle_2
            else:
                Angle_2 = [0,0,0,0,0,0,0]

            if Angle_3 is not None:
                Angle_3 = Angle_3
            else:
                Angle_3 = [0,0,0,0,0,0,0]


            if Coordinate is not None:
                Coordinate = Coordinate
            else:
                Coordinate = [0,0,0,0,0,0,0]

            if Coordinate_2 is not None:
                Coordinate_2 = Coordinate_2
            else:
                Coordinate_2 = [0,0,0,0,0,0,0]
            
            if Coordinate_3 is not None:
                Coordinate_3 = Coordinate_3
            else:
                Coordinate_3 = [0,0,0,0,0,0,0]
            
            self.IO = process_master.master_.execute(1,cst.READ_HOLDING_REGISTERS,starting_address=37,quantity_of_x=10)
            io = self.IO
            
            self.update_modbus_variables(GB_dict, GI_dict, GD_dict, GP_dict)

            # ### 更新全局变量--待发送

            pose_ModbusVar = [value_dict["value"] for key, value_dict in GP_dict.items()]  
            bool_ModbusVar = [value_dict["value"] for key, value_dict in GB_dict.items()]  
            int_ModbusVar = [value_dict["value"] for key, value_dict in GI_dict.items()]  
            float_ModbusVar = [value_dict["value"] for key, value_dict in GD_dict.items()]  
            ### 消息体
            message_body = self.create_message_body(controller, type_robot1, state_robot1, Angle, Coordinate, type_robot2, state_robot2, Angle_2, Coordinate_2, 
                                                    type_robot3, state_robot3, Angle_3, Coordinate_3,io,
                                                    pose_ModbusVar, bool_ModbusVar, int_ModbusVar, float_ModbusVar)
            
            ### 消息体长度
            data_size = len(message_body)
            ### 控制器类型：实体控制器
            packet_type = "mainStatus" 
            ### 构建数据包 
            packet = self.build_packet(data_size, packet_type, message_body)
            # print(f"---send_data is {packet}---")
            self.client_socket.sendall(packet)
    
        except Exception as e:
            print(f"send_process data is error is {e}")





